/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "ProximitySetup.hpp"

#include "CollisionSetup.hpp"

#include <rw/core/StringUtil.hpp>
#include <rw/models/Object.hpp>
#include <rw/models/WorkCell.hpp>

using namespace rw::core;
using namespace rw::proximity;

ProximitySetup::ProximitySetup () :
    _useIncludeAll (true), _useExcludeStaticPairs (true), _loadedFromFile (false)
{}

ProximitySetup::ProximitySetup (const CollisionSetup& csetup) :
    _useIncludeAll (true), _useExcludeStaticPairs (true), _loadedFromFile (false)
{
    for (rw::core::StringPair pair : csetup.getExcludeList ()) {
        addProximitySetupRule (ProximitySetupRule::makeExclude (pair.first, pair.second));
    }
    _useExcludeStaticPairs = csetup.excludeStaticPairs ();
}

ProximitySetup::ProximitySetup (const std::vector< ProximitySetupRule >& rules)
{
    for (const ProximitySetupRule& rule : rules) {
        addProximitySetupRule (rule);
    }
};

void ProximitySetup::addProximitySetupRule (const ProximitySetupRule& rule)
{
    _rules.push_back (rule);
}

void ProximitySetup::removeProximitySetupRule (const ProximitySetupRule& rule)
{
    for (std::vector< ProximitySetupRule >::iterator it = _rules.begin (); it != _rules.end ();
         ++it) {
        ProximitySetupRule& r = *it;
        if (r == rule) {
            _rules.erase (it);
            break;
        }
    }
}

ProximitySetup ProximitySetup::get (rw::models::WorkCell::Ptr wc)
{
    return get (wc->getWorldFrame ()->getPropertyMap ());
}

ProximitySetup ProximitySetup::get (const rw::models::WorkCell& wc)
{
    return get (wc.getWorldFrame ()->getPropertyMap ());
}

ProximitySetup ProximitySetup::get (const rw::core::PropertyMap& map)
{
    return map.get< ProximitySetup > ("ProximitySetup", ProximitySetup ());
}

void ProximitySetup::set (const ProximitySetup& setup, rw::models::WorkCell::Ptr wc)
{
    set (setup, wc->getWorldFrame ()->getPropertyMap ());
}

void ProximitySetup::set (const ProximitySetup& setup, rw::core::PropertyMap& map)
{
    map.addForce< ProximitySetup > ("ProximitySetup", "setup for proximity checking", setup);
}
